#include "Matrix/LB_Matrix.h"
#include "Mesh/_Func_.h"
#include "tools/intrbtree.h"
#include "tools_node.h"
#include "tools_vector.h"
#include<Algorithm/mesh_smoothing.h>
#include<Matrix/LB_Sparse_Matrix.h>
#include <cstring>

#define quote lib_cell_quote


// static void tool_mesh_smoothing_taubin(LB_Sparse_Matrix* lsm,
//     double*x,double*xn,int len1,double l)
// {
//     lb_sparse_matrix_mult_array(lsm,
//         x,xn);

//     for(int i=0;i<len1;i++)
//     {
//         xn[i]=l*xn[i]+(1-l)*x[i];
//     }
// }


void mesh_smoothing_taubin(Mesh*m,int include_bound,double l1,double l2,int iter)
{
    Node* n=NULL;
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        if(!include_bound&&m->vertex_is_boundary(m,*vit))
        {
            continue;
        }
        n=node_overlying(n,quote(vit));
    }

    mesh_smoothing_taubin_noden(n,
        l1,l2,iter);
    free_node(n);
}

static void tool_mesh_smoothing_taubin_node(Node* n,double l)
{
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        // Node* nodev=m->vv_begin(m,*vit);
        template_v*vv=(template_v*)(nit->value);
        LB_Vector_voidptr vec=Mesh_vv_beginn(NULL, *vv);
        if(vec.size<=0){continue;}
        // Node*nodev= Mesh_vv_begin(NULL,*(vv));

        // if(nodev==NULL){continue;}
        double dir[3]={0};double sum=0;
        // for(Node*nit1=nodev;nit1!=NULL;nit1=(Node*)(nit1->Next))
        for(int i=0;i<vec.size;i++)
        {
            // template_v* v=(template_v*)(nit1->value);
            template_v* v=(template_v*)(*(vec.at(&vec,i)));
            dir[0]+=(v->point[0]-vv->point[0]);
            dir[1]+=(v->point[1]-vv->point[1]);
            dir[2]+=(v->point[2]-vv->point[2]);
            sum+=1.0;
        }
        // free_node(nodev);

        dir[0]/=sum;dir[1]/=sum;dir[2]/=sum;
        vv->point[0]+=(dir[0]*l);
        vv->point[1]+=(dir[1]*l);
        vv->point[2]+=(dir[2]*l);
    }

}


void mesh_smoothing_taubin_node(Node* n,double l1,double l2,int iter)
{

    for(int i=0;i<iter;i++)
    {
        tool_mesh_smoothing_taubin_node(n,l1);
        tool_mesh_smoothing_taubin_node(n,l2);
        // num++;
    }

}



static inline void mesh_smoothing_taubin_noden_tool(LB_Sparse_Matrix* lsm,
    double* b,double*x,double*xn,int len1,double l )
{
    lb_sparse_matrix_mult_array(lsm,
        x,xn);

    for(int i=0;i<len1;i++)
    {
        xn[i]=l*(xn[i]+b[i])+(1-l)*x[i];
    }
    // array_plus(xn,b,xn,len1);
    // array_mult_scala(xn,l,xn,len1);
    // array_mult_scala(x,1-l,x,len1);
    // array_plus(xn,x,xn,len1);
}

void mesh_smoothing_taubin_noden(Node* n,
    double l1,double l2,int iter)
{
    // int len=node_size(n);
    // double* re=(double*)malloc(sizeof(double)*len);
    // memset(re,0,sizeof(double)*len);

    Int_RB_Tree* v_inner_id=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(v_inner_id);

    // Int_RB_Tree* v_out_id=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    // int_rb_tree_init(v_out_id);

    int i1=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        int* value=(int*)malloc(sizeof(int));
        v_inner_id->insert(v_inner_id,v->id,value);

        *value=i1;
        i1++;
    }

    int len1=v_inner_id->size;
    LB_Sparse_Matrix *lsm = (LB_Sparse_Matrix *)malloc(sizeof(LB_Sparse_Matrix));
    lb_sparse_matrix_initn(lsm, len1, len1);
    double** b=(double**)malloc(sizeof(double*)*3);
    double** x=(double**)malloc(sizeof(double*)*3);
    double** xn=(double**)malloc(sizeof(double*)*3);

    for(int i=0;i<3;i++)
    {
        b[i]=(double*)malloc(sizeof(double)*len1);
        memset(b[i],0,sizeof(double)*len1);
        x[i]=(double*)malloc(sizeof(double)*len1);
        memset(x[i],0,sizeof(double)*len1);
        xn[i]=(double*)malloc(sizeof(double)*len1);
        memset(xn[i],0,sizeof(double)*len1);
    }


    Node* nit=n;
    for(int i=0;i<len1;i++,nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        // nit=(Node*)(nit->Next);

        LB_Vector_voidptr vec= Mesh_vv_beginn(NULL,*v);
        if(vec.size==0){continue;}
        // Node* nodev = Mesh_vv_begin(NULL,*v);
        // if(nodev==NULL){continue;}
        int size_node=vec.size+1;
        // int size_node=node_size(nodev)+1;
        // lb_sparse_matrix_setrowsize(lsm,id,size_node);
        lsm->data[i]=(SparseMatrixEntry*)
            malloc(sizeof(SparseMatrixEntry)*size_node);
        double sum_weight=0;
        // for(Node nit1=*nodev;*nit1!=NULL;nit1++)
        for(int j=0;j<vec.size;j++)
        {
            // template_v*vv=(template_v*)(*nit1);
            template_v*vv=(template_v*)(*(vec.at(&vec,j)));
            int* value=(int*)(v_inner_id->find(
                v_inner_id,vv->id));
            sum_weight+=1.0;
            if(value==NULL)
            {
                b[0][i]+=vv->point[0];
                b[1][i]+=vv->point[1];
                b[2][i]+=vv->point[2];
            }
            else
            {
                lsm->data[i][lsm->rowSizes[i]].i=*value;
                lsm->data[i][lsm->rowSizes[i]].v=1.0;
                lsm->rowSizes[i]++;
            }
        }
        // free_node(nodev);
        vec.clear(&vec);
        x[0][i]=v->point[0];
        x[1][i]=v->point[1];
        x[2][i]=v->point[2];
        b[0][i]/=sum_weight;
        b[1][i]/=sum_weight;
        b[2][i]/=sum_weight;
        lsm->data[i]=(SparseMatrixEntry*)realloc(
            lsm->data[i],sizeof(SparseMatrixEntry)*lsm->rowSizes[i]);
        for(int k=0;k<lsm->rowSizes[i];k++)
        {
            lsm->data[i][k].v/=sum_weight;
        }
    }


    for(int i=0;i<iter;i++)
    {
        for(int j=0;j<3;j++)
        {
            mesh_smoothing_taubin_noden_tool(lsm,
                b[j],x[j],xn[j],len1,l1 );
        }

        double** tempx=x;
        x=xn;
        xn=tempx;
        for(int j=0;j<3;j++)
        {
            mesh_smoothing_taubin_noden_tool(lsm,
                b[j],x[j],xn[j],len1,l2 );
        }

        tempx=x;
        x=xn;
        xn=tempx;
    }

    i1=0;
    for(nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        v->point[0]=x[0][i1];
        v->point[1]=x[1][i1];
        v->point[2]=x[2][i1];
        i1++;
    }
    lb_sparse_matrix_free(lsm);
    for(int i=0;i<3;i++)
    {
        free(b[i]);free(x[i]);free(xn[i]);
    }
    free(b);free(x);free(xn);
    int_rb_tree_free_value(v_inner_id);
    // int_rb_tree_free_value(v_out_id);
    // int_rb_tree_free(v_out_id);
    int_rb_tree_free(v_inner_id);
    // return re;
}


static Int_RB_Tree* my_get_impact_scope_vertices(
    Node* nodev,int impact_scope)
{
    Int_RB_Tree* treev=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(treev);
    LB_Vector_voidptr vec1,vec2;
    lb_vector_init_voidptr(&vec1);
    lb_vector_init_voidptr(&vec2);
    LB_Vector_voidptr* vecp1=&vec1,*vecp2=&vec2;
    // Node* tempn=NULL;
    for(Node* nit=nodev;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        vecp1->push_back(vecp1,v);
        treev->insert(treev,v->id,v);
    }

    for(int num=0;num<impact_scope;num++)
    {
        vecp2->clear(vecp2);
        for(int i=0;i<vecp1->size;i++)
        {
            template_v*v=(template_v*)(*(vecp1->at(vecp1,i)));
            LB_Vector_voidptr vec= Mesh_vv_beginn(NULL,*v);
            for(int j=0;j<vec.size;j++)
            {
                template_v*vv=(template_v*)(*(vec.at(&vec,j)));
                if(treev->find(treev,vv->id)==NULL)
                {
                    treev->insert(treev,vv->id,vv);
                    vecp2->push_back(vecp2,vv);
                }
            }
            vec.clear(&vec);
        }
        vecp1->clear(vecp1);
        LB_Vector_voidptr* tempp=vecp1;
        vecp1=vecp2;
        vecp2=tempp;
    }

    vec1.clear(&vec1);
    vec2.clear(&vec2);

    return treev;
}


static void compute_cell_area_normal(template_c*c,double* normal)
{
    if(c==NULL)
    {
        return ;
    }
    template_hf* hf=(template_hf*)(c->halffaces[0]);
    template_v* v=NULL;
    for(int i=0;i<c->vertices_size;i++)
    {
        if(c->vertices[i]!=hf->vertices[0]&&c->vertices[i]!=hf->vertices[1])
        {
            v=c->vertices[i];
            break;
        }
    }

    if(v==NULL)
    {return ;}

    double p1[3]={hf->vertices[0]->point[0]-v->point[0],hf->vertices[0]->point[1]-v->point[1],
        hf->vertices[0]->point[2]-v->point[2]};

    double p2[3]={hf->vertices[1]->point[0]-v->point[0],hf->vertices[1]->point[1]-v->point[1],
        hf->vertices[1]->point[2]-v->point[2]};
    out_productn(p1, p2, normal);
    // double*re=out_product(p1,p2);
    // normalize(normal,3);
    // return re;
}

// static inline double compute_triangle_area(double* p1,double* p2,double* p3)
// {
//     double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};

//     double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

//     double q[3]={q1[1]*q2[2]-q1[2]*q2[1],-q1[0]*q2[2]+q1[2]*q2[0],q1[0]*q2[1]-q1[1]*q2[0]};
//     double re=SAFE_SQRT(q[0]*q[0]+q[1]*q[1]+q[2]*q[2])/2.0;

//     return re;
// }

// static inline double compute_cell_area(template_c*c)
// {
//     if(c==NULL){return 0;}
//     template_v* v=c->vertices[0];
//     double re=0;
//     //for(Node* nit=c->halffaces;nit!=NULL;nit=(Node*)(nit->Next))
//     for(auto chfit=Mesh_chf_begin(NULL,*c);chfit!=Mesh_chf_end(NULL,*c);chfit++)
//     {
//         template_hf* hf=quote(chfit);
//         if(hf->vertices[0]==v||hf->vertices[1]==v )
//         {
//             continue;
//         }
//         re+=compute_triangle_area(hf->vertices[0]->point,hf->vertices[1]->point,v->point);
//     }

//     return re;
// }
static void compute_vertex_normal(template_v*v,double*normal)
{
    // LB_Vector_voidptr vec=Mesh_vv_beginn(NULL,*v);
    memset(normal, 0, sizeof(double)*3);
    for(int i=0;i<v->cellsn.size;i++)
    {
        double normal1[3]={0};
        template_c* c=(template_c*)(*(lb_vector1_at_voidptr(&(v->cellsn),i)) );
        compute_cell_area_normal(c,normal1);
        normal[0]+=normal1[0];
        normal[1]+=normal1[1];
        normal[2]+=normal1[2];
    }
    normalize(normal, 3);

}

void simple_carving_smooth(Node* nodev,
    int impact_scope,int iter_num )
{
    Int_RB_Tree* treev=my_get_impact_scope_vertices(
        nodev,impact_scope);
    int len=node_size(nodev);
    double** normals=(double**)malloc(len*sizeof(double*));
    memset(normals, 0, len*sizeof(double*));
    int i=0;
    for(Node* nit=nodev;nit!=NULL;i++,nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        double *normal=(double*)malloc(sizeof(double)*3);
        memset(normal,0,sizeof(double)*3);
        compute_vertex_normal(v,normal);
        normals[i]=normal;
    }
    i=0;
    for(Node* nit=nodev;nit!=NULL;i++,nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        double  strength=*((double*)(nit->traits));
        v->point[0]+=normals[i][0]*strength;
        v->point[1]+=normals[i][1]*strength;
        v->point[2]+=normals[i][2]*strength;
    }

    Node* nodev_s=NULL;
    for(auto it=treev->begin(treev);it.it!=NULL;it++)
    {
        nodev_s=node_overlying(nodev_s, it.second);
    }

    int_rb_tree_free(treev);

    mesh_smoothing_taubin_noden(nodev_s,
        0.5,-0.25,iter_num);

    free_node(nodev_s);
    for(i=0;i<len;i++)
    {
        free(normals[i]);
    }
    free(normals);
}



double* compute_once_surface_dis_map(Mesh* m,
    Node * sources_v)
{
    Int_RB_Tree*treev=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(treev);
    for(Node* nit=sources_v;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        treev->insert(treev,v->id,v);

    }

    Int_RB_Tree* v_inner_id=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(v_inner_id);
    Node* others_v=NULL;
    int id=0;
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++ )
    {
        if(treev->find(treev,quote(vit)->id)==NULL)
        {
            int *value=(int*)malloc(sizeof(int));
            *value=id;
            v_inner_id->insert(v_inner_id,quote(vit)->id,
                value);

            others_v=node_overlying(others_v,quote(vit));

            id++;
        }
    }

    double* re=(double*)malloc(sizeof(double)*id);
    double* b=(double*)malloc(sizeof(double)*id);
    LB_Sparse_Matrix *lsm = (LB_Sparse_Matrix *)malloc(sizeof(LB_Sparse_Matrix));
    lb_sparse_matrix_initn(lsm, id, id);

    int i=0;
    for(Node* nit = others_v; nit != NULL; i++, nit = (Node*)(nit->Next) )
    {
        re[i]=-1;b[i]=-1;
        template_v* v=(template_v*)(nit->value);

        LB_Vector_voidptr vec= Mesh_vv_beginn(NULL,*v);
        if(vec.size==0){continue;}

        int size_node=vec.size+1;

        lsm->data[i]=(SparseMatrixEntry*)
            malloc(sizeof(SparseMatrixEntry)*size_node);

        for(int j=0;j<vec.size;j++)
        {
            // template_v*vv=(template_v*)(*nit1);
            template_v*vv=(template_v*)(*(vec.at(&vec,j)));
            int* value=(int*)(v_inner_id->find(
                v_inner_id,vv->id));
            if(value==NULL)
            {
                double dis=distance_of_two_points(vv->point,v->point,3);
                if(b[i]<0||dis<b[i])
                {
                    b[i]=dis;
                }
            }
            else
            {
                lsm->data[i][lsm->rowSizes[i]].i=*value;
                lsm->data[i][lsm->rowSizes[i]].v=
                    distance_of_two_points(vv->point,v->point,3);
                lsm->rowSizes[i]++;
            }
        }

        vec.clear(&vec);
        lsm->data[i]=(SparseMatrixEntry*)realloc(
            lsm->data[i],sizeof(SparseMatrixEntry)*lsm->rowSizes[i]);

    }

    // for(int i=0;i<iter;i++)
    // {

    // }


    free(b);
    lb_sparse_matrix_free(lsm);

    free_node(others_v);
    int_rb_tree_free(treev);
    int_rb_tree_free_value(v_inner_id);
    int_rb_tree_free(v_inner_id);
    return re;
}

// void mesh_smoothing_taubin_noden(Node* n,
//     double l1,double l2,int iter)
